#pragma once
#include <Eigen/Dense>
    double step_rate = 0.1;
    Eigen::Vector2d A = {0, 0};
    Eigen::Vector2d B = {0, 0};
    Eigen::Vector2d C = {0, 0};
    Eigen::Matrix3d H = {1, 0, 0, 1, 0, 0, 1, 0, 0};
namespace vehicle_kinematics
{
    /**
     * 为了简化程序，在此规定：
     *  wheel舵轮绕Z轴方向旋转,为X轴与parentX轴夹角
     *  wheel前进方向绕Y轴旋转
     */
    class Position2D
    {
    private:
        /* data */
        double x_;
        double y_;

    public:
        Position2D(){};
        Position2D(double x, double y)
        {
            x_ = x;
            y_ = y;
        };
        ~Position2D(){};

    public:
        void setX(double x) { x_ = x; };
        double getX() { return x_ };
        void setY(double y) { y_ = y; };
        double getY() { return y_ };
        void setPostion(double x, double y)
        {
            x_ = x;
            y_ = y;
            z_ = z;
        };
        void getVector2d(Eigen::Vector2d &position) {
            position[0] = x_;
            position[1] = y_;
        };
        void setVector2d(const Eigen::Vector2d &position)
        {
            x_ = position[0];
            y_ = position[1];
        };
    };

    class Quaternion
    {
    private:
        /* data */
        double x_;
        double y_;
        double z_;
        double w_;

    public:
        Quaternion()
        {
            x_ = 0;
            y_ = 0;
            z_ = 0;
            w_ = 1;
        };
        Quaternion(double x, double y, double z, double w)
        {
            x_ = x;
            y_ = y;
            z_ = z;
            w_ = w;
        };
        ~Quaternion(){};

    public:
        void setX(double x) { x_ = x };
        void setY(double x) { y_ = x };
        void setZ(double x) { z_ = x };
        void setW(double x) { w_ = x };
        double getX() { retutn x_; };
        double getY() { retutn y_; };
        double getZ() { retutn z_; };
        double getW() { retutn w_; };

        void getQuaternion(Eigen::Quaternion &q)
        {
            Eigen::Quaternion qq(w_, x_, y_, z_);
            q = qq.norm();
        };

        void setQuaternion(const Eigen::Quaternion &q)
        {
            w_ = q.w();
            x_ = q.x();
            y_ = q.y();
            z_ = q.z();
        };
    };

    class VehicleWheelState2D
    {
    private:
        /* data */
        std::string frame_id_;
        dobule steer_angle_;
        double steer_rotate_speed_;  // 轮子旋转速度rad/s
        double wheel_radius_;
        Position2D position_;

    public:
        VehicleWheelState2D(std::string frame_id,
                          double steer_angle,
                          double steer_rotate_speed,
                          dobule wheel_radius,
                          Position2D position)
        {
            frame_id_ = frame_id;
            steer_angle_ = steer_angle;
            steer_rotate_speed_ = steer_rotate_speed;
            position_ = position;
            wheel_radius_ = wheel_radius;
        };
        VehicleWheelState2D(){
            frame_id_ = "";
            steer_angle_ = 0;
            steer_rotate_speed_ = 0;
        };
        ~VehicleWheelState2D();
    public:
        void setFrameId(std::string frame_id){frame_id_ = frame_id;}
        void setSteerAngle(double steer_angle){steer_angle_ = steer_angle;}
        void setSteerRotateSpeed(double steer_rotate_speed){fsteer_rotate_speed_ = steer_rotate_speed;}
    public:
        std::string getFrameId(){return frame_id_;};
        double getSteerAngle(){return steer_angle_};
        double getSteerRotateSpeed(){return steer_rotate_speed_;};
        Position2D getPosition(){return position_};
        double getWheelRadius(){return wheel_radius_};
        double getWheelLinearSpeed(){return wheel_radius_ * steer_rotate_speed_};
    };

    VehicleWheelState2D::VehicleWheelState2D(std::string frame_id,
                                         double steer_angle,
                                         double steer_rotate_speed)
    {
        frame_id_ = frame_id;
        steer_angle_ = steer_angle;
        steer_rotate_speed_ = steer_rotate_speed;
    }

    VehicleWheelState2D::~VehicleWheelState2D()
    {
    }

    class VehicleWheel
    {
    private:
        /* data: unit meter seconds */
        std::string parent_frame_id_;
        std::string frame_id_;
        Position2D position_;
        double yaw_orientaion_;

        double wheel_radius_;
        double wheel_width_;

    private:
        /* 物理限制 */
        double max_steer_angle_;
        double min_steer_angle_;

    public:
        VehicleWheel(
            const std::string &parent_frame,
            const std::string &frame_id,
            const Position2D &position,
            const double &orientation,
            const double &wheel_radius,
            const double &wheel_width,
            const double &max_steer_angle,
            const double &min_steer_angle) : parent_frame_id_(parent_frame),
                                             frame_id_(frame_id),
                                             position_(position),
                                             orientation_(orientation),
                                             wheel_radius_(wheel_radius),
                                             wheel_width_(wheel_width),
                                             max_steer_angle_(max_steer_angle),
                                             min_steer_angle_(min_steer_angle),
                                             {
                                             };
        ~VehicleWheel(){};
    public:
        void getFrameId(){return frame_id_;};
        void getParentFrameId(){return parent_frame_id_;};
    public:
        VehicleWheelState2D calcWheelState(
            const Eigen::Vector2d &instant_center_position,
            const Eigen::Vector2d &instant_center_linear,
            const Eigen::Vector2d &instant_center_angular
            )
        {
            // 初始化轮子
            VehicleWheelState2D ws(getFrameId(), 0.0, 0.0, wheel_radius_, position_);

            // 求质点相对于瞬心的坐标
            Eigen::Vector2d mass_to_instant;
            position_.getVector2d(mass_to_instant);
            
            // 如果瞬心无穷远
            for(auto &p : instant_center_position)
            {
                if (std::isnan(p))
                {
                    ws.setSteerRotateSpeed(instant_center_linear.norm() / wheel_radius_);
                    ws.setSteerAngle(std::atan2(instant_center_linear[1], instant_center_linear[0]));
                    return ws;
                }
            }

            // 如果瞬心不是无穷远
            mass_to_instant = mass_to_instant - instant_center_position;
            Eigen::Vector3d wheel_speed = instant_center_angular.cross(mass_to_instant);
            ws.setSteerAngle(std::atan2(wheel_speed[1], wheel_speed[0]));
            ws.setSteerRotateSpeed(wheel_speed.norm() / wheel_radius_);
            return ws;
        };
    };

}